import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
import logging
import pandas as pd
from datetime import datetime


# 设置支持中文的字体
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

# 设置日志
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(levelname)s - %(message)s',
    handlers=[
        logging.FileHandler(f"uav_simulation_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log"),
        logging.StreamHandler()
    ]
)
logger = logging.getLogger('UAV_Simulation')


class UAVDynamics:
    def __init__(self, dt=0.01):
        # 状态: [四元数w, 四元数x, 四元数y, 四元数z, 角速度x, 角速度y, 角速度z]
        self.state = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        self.dt = dt

        # 物理参数 (典型四旋翼无人机)
        self.mass = 1.0  # kg
        self.Ixx = 0.01  # kg*m^2
        self.Iyy = 0.01  # kg*m^2
        self.Izz = 0.02  # kg*m^2
        self.inertia = np.diag([self.Ixx, self.Iyy, self.Izz])
        self.inv_inertia = np.linalg.inv(self.inertia)

        # 空气动力学阻尼
        self.damping = np.diag([0.1, 0.1, 0.1])

        logger.info("无人机动力学模型初始化: dt=%.3f, 转动惯量=diag(%.3f, %.3f, %.3f)",
                    dt, self.Ixx, self.Iyy, self.Izz)

    def update(self, torque):
        # 提取当前状态
        q = self.state[:4]  # 四元数
        w = self.state[4:]  # 角速度

        # 归一化四元数
        q = q / np.linalg.norm(q)

        # 计算角加速度 (欧拉旋转方程)
        angular_acc = self.inv_inertia @ (torque - np.cross(w, self.inertia @ w) - self.damping @ w)

        # 更新角速度
        w_new = w + angular_acc * self.dt

        # 使用精确的四元数积分方法
        omega_norm = np.linalg.norm(w_new)
        if omega_norm > 1e-12:
            axis = w_new / omega_norm
            angle = omega_norm * self.dt
            dq = np.array([
                np.cos(angle / 2),
                axis[0] * np.sin(angle / 2),
                axis[1] * np.sin(angle / 2),
                axis[2] * np.sin(angle / 2)
            ])
            q_new = self.quaternion_multiply(q, dq)
        else:
            q_new = q.copy()

        # 归一化四元数
        q_new = q_new / np.linalg.norm(q_new)

        # 更新状态
        self.state = np.concatenate([q_new, w_new])

        return self.state.copy()

    def quaternion_multiply(self, q1, q2):
        # 四元数乘法
        w1, x1, y1, z1 = q1
        w2, x2, y2, z2 = q2
        return np.array([
            w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
            w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
            w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
            w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
        ])


class AdvancedEKF:
    def __init__(self, dt):
        self.dt = dt

        # 状态: [q0, q1, q2, q3, wx, wy, wz, bx, by, bz] (添加陀螺仪偏置估计)
        self.state_dim = 10
        self.measurement_dim = 6  # 加速度计(3) + 磁力计(3)

        # 过程噪声协方差 - 偏置的过程噪声应该很小
        self.Q = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4, 1e-8, 1e-8, 1e-8])

        # 测量噪声协方差
        self.R = np.diag([0.3, 0.3, 0.3, 0.5, 0.5, 0.5])

        # 初始状态协方差
        self.P = np.eye(self.state_dim) * 0.1

        # 初始状态 (单位四元数，零角速度，零偏置)
        self.x = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

        # 重力向量 (NED坐标系: 北-东-地)
        self.gravity = np.array([0, 0, 9.81])

        # 地磁场向量 (假设指向北)
        self.magnetic_field = np.array([0.5, 0, 0.4])

        # 线性加速度检测历史
        self.linear_acc_detection = []

        # 滤波器健康状态
        self.healthy = True
        self.converged = False
        self.reset_count = 0

        logger.info("高级EKF初始化完成")

    def predict(self, gyro_measurement):
        # 提取状态分量
        q = self.x[:4]
        w = self.x[4:7]
        b = self.x[7:10]  # 陀螺仪偏置

        # 归一化四元数
        q = q / np.linalg.norm(q)

        # 使用偏置校正后的陀螺仪测量值
        corrected_gyro = gyro_measurement - b

        # 使用陀螺仪测量值作为角速度预测
        w_pred = w.copy()

        # 使用精确的四元数积分方法
        omega_norm = np.linalg.norm(corrected_gyro)
        if omega_norm > 1e-12:
            axis = corrected_gyro / omega_norm
            angle = omega_norm * self.dt
            dq = np.array([
                np.cos(angle / 2),
                axis[0] * np.sin(angle / 2),
                axis[1] * np.sin(angle / 2),
                axis[2] * np.sin(angle / 2)
            ])
            q_pred = self.quaternion_multiply(q, dq)
        else:
            q_pred = q.copy()

        # 归一化四元数
        q_pred = q_pred / np.linalg.norm(q_pred)

        # 偏置预测 (假设恒定)
        b_pred = b.copy()

        # 更新状态
        self.x = np.concatenate([q_pred, w_pred, b_pred])

        # 简化过程雅可比矩阵 - 使用近似值
        F = np.eye(self.state_dim)

        # 预测协方差
        self.P = F @ self.P @ F.T + self.Q

        return self.x

    def update(self, accel_measurement, mag_measurement):
        # 线性加速度检测
        accel_norm = np.linalg.norm(accel_measurement)
        gravity_norm = np.linalg.norm(self.gravity)
        accel_diff = abs(accel_norm - gravity_norm)
        self.linear_acc_detection.append(accel_diff)

        # 动态调整测量噪声 (如果检测到线性加速度)
        R_temp = self.R.copy()
        if accel_diff > 1.0:  # 1 m/s^2阈值
            R_temp[:3, :3] = np.diag([1.0, 1.0, 1.0])  # 临时增加加速度计噪声

        # 预测测量值
        q = self.x[:4]
        q = q / np.linalg.norm(q)

        # 计算旋转矩阵
        rot = Rotation.from_quat([q[1], q[2], q[3], q[0]])
        R = rot.as_matrix()

        # 预期测量值
        expected_accel = R.T @ self.gravity
        expected_mag = R.T @ self.magnetic_field

        # 实际测量值
        z = np.concatenate([accel_measurement, mag_measurement])
        expected_z = np.concatenate([expected_accel, expected_mag])

        # 数值计算雅可比矩阵
        H = self.numerical_jacobian(q)

        # 卡尔曼增益
        S = H @ self.P @ H.T + R_temp
        try:
            K = self.P @ H.T @ np.linalg.inv(S)
        except np.linalg.LinAlgError:
            logger.warning("卡尔曼增益计算中的矩阵求逆失败")
            return self.x

        # 状态更新
        y = z - expected_z  # 创新序列
        self.x = self.x + K @ y

        # 归一化四元数
        self.x[:4] = self.x[:4] / np.linalg.norm(self.x[:4])

        # 协方差更新 (Joseph形式，保证正定性)
        I = np.eye(self.state_dim)
        self.P = (I - K @ H) @ self.P @ (I - K @ H).T + K @ R_temp @ K.T

        # 检查滤波器健康状态
        self.check_health()

        return self.x

    def check_health(self):
        # 检查协方差矩阵
        if np.any(np.diag(self.P) > 10.0) or np.any(np.isnan(self.P)):
            logger.warning("滤波器协方差异常，正在重置...")
            self.reset()
            self.healthy = False
        else:
            self.healthy = True

        # 检查四元数
        q_norm = np.linalg.norm(self.x[:4])
        if q_norm < 0.9 or q_norm > 1.1:
            logger.warning("四元数范数异常: %.4f, 重新归一化...", q_norm)
            self.x[:4] = self.x[:4] / q_norm

        # 检查收敛
        if np.all(np.diag(self.P)[:4] < 0.01) and not self.converged:
            self.converged = True
            logger.info("滤波器已收敛")

    def reset(self):
        # 重置滤波器
        self.P = np.eye(self.state_dim) * 0.1
        self.x = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        self.reset_count += 1
        logger.info("滤波器已重置 (第%d次)", self.reset_count)

    def numerical_jacobian(self, q, epsilon=1e-6):
        # 数值计算雅可比矩阵，避免解析推导错误
        H = np.zeros((self.measurement_dim, self.state_dim))

        # 创建扰动函数
        def measurement_model(x):
            q_temp = x[:4] / np.linalg.norm(x[:4])
            rot = Rotation.from_quat([q_temp[1], q_temp[2], q_temp[3], q_temp[0]])
            R = rot.as_matrix()
            expected_accel = R.T @ self.gravity
            expected_mag = R.T @ self.magnetic_field
            return np.concatenate([expected_accel, expected_mag])

        # 当前状态
        x0 = np.concatenate([q, self.x[4:]])

        # 数值计算雅可比矩阵
        for i in range(self.state_dim):
            dx = np.zeros(self.state_dim)
            dx[i] = epsilon
            H[:, i] = (measurement_model(x0 + dx) - measurement_model(x0 - dx)) / (2 * epsilon)

        return H

    def quaternion_multiply(self, q1, q2):
        # 四元数乘法
        w1, x1, y1, z1 = q1
        w2, x2, y2, z2 = q2
        return np.array([
            w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
            w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
            w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
            w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
        ])


# 模拟参数
dt = 0.01  # 时间步长
total_time = 10  # 总模拟时间
steps = int(total_time / dt)  # 总步数

logger.info("开始模拟: dt=%.3f, total_time=%.1f, steps=%d", dt, total_time, steps)

# 创建无人机模型
uav = UAVDynamics(dt)

# 创建高级EKF
ekf = AdvancedEKF(dt)

# 数据存储
time_points = np.arange(0, total_time, dt)

# 真实状态
true_quaternions = np.zeros((steps, 4))
true_angular_velocities = np.zeros((steps, 3))

# 传感器测量
gyro_measurements = np.zeros((steps, 3))
accel_measurements = np.zeros((steps, 3))
mag_measurements = np.zeros((steps, 3))

# EKF估计
ekf_quaternions = np.zeros((steps, 4))
ekf_angular_velocities = np.zeros((steps, 3))
ekf_biases = np.zeros((steps, 3))

# 欧拉角用于可视化
true_euler_angles = np.zeros((steps, 3))
ekf_euler_angles = np.zeros((steps, 3))

# 性能监控
estimation_errors = {
    'roll': [],
    'pitch': [],
    'yaw': [],
    'roll_rate': [],
    'pitch_rate': [],
    'yaw_rate': []
}

# 模拟循环
for t in range(steps):
    current_time = t * dt

    # 生成控制扭矩 (较小的幅度，避免剧烈运动)
    torque = np.array([
        0.02 * np.sin(0.5 * current_time),  # 横滚扭矩
        0.015 * np.cos(0.7 * current_time),  # 俯仰扭矩
        0.01 * np.sin(0.3 * current_time)  # 偏航扭矩
    ])

    # 更新无人机真实状态
    true_state = uav.update(torque)
    true_quaternions[t] = true_state[:4]
    true_angular_velocities[t] = true_state[4:]

    # 转换四元数为欧拉角用于可视化
    true_rot = Rotation.from_quat([true_state[1], true_state[2], true_state[3], true_state[0]])
    true_euler_angles[t] = true_rot.as_euler('xyz', degrees=True)

    # 模拟传感器测量 (添加噪声和偏置)
    # 陀螺仪 (角速度 + 偏置 + 噪声)
    gyro_bias = np.array([0.01, -0.01, 0.005])  # 恒定偏置
    gyro_noise = np.random.normal(0, 0.01, 3)  # 随机噪声
    gyro_measurements[t] = true_angular_velocities[t] + gyro_bias + gyro_noise

    # 加速度计 (重力 + 线性加速度 + 噪声)
    rot = Rotation.from_quat([true_state[1], true_state[2], true_state[3], true_state[0]])
    gravity_body = rot.as_matrix().T @ np.array([0, 0, 9.81])

    # 添加一些线性加速度 (模拟运动)
    linear_acc = np.array([
        0.05 * np.sin(0.8 * current_time),
        0.03 * np.cos(0.6 * current_time),
        0.08 * np.sin(0.4 * current_time)
    ])

    accel_noise = np.random.normal(0, 0.1, 3)
    accel_measurements[t] = gravity_body + linear_acc + accel_noise

    # 磁力计 (地球磁场在机体坐标系 + 噪声)
    mag_body = rot.as_matrix().T @ np.array([0.5, 0, 0.4])
    mag_noise = np.random.normal(0, 0.1, 3)
    mag_measurements[t] = mag_body + mag_noise

    # EKF预测步骤 (使用陀螺仪测量)
    ekf.predict(gyro_measurements[t])

    # EKF更新步骤 (使用加速度计和磁力计测量)
    ekf_estimate = ekf.update(accel_measurements[t], mag_measurements[t])
    ekf_quaternions[t] = ekf_estimate[:4]
    ekf_angular_velocities[t] = ekf_estimate[4:7]
    ekf_biases[t] = ekf_estimate[7:10]

    # 转换EKF四元数为欧拉角
    ekf_rot = Rotation.from_quat([ekf_estimate[1], ekf_estimate[2], ekf_estimate[3], ekf_estimate[0]])
    ekf_euler_angles[t] = ekf_rot.as_euler('xyz', degrees=True)

    # 计算并记录误差
    if t > 10:  # 跳过前几步以允许滤波器初始化
        roll_error = abs(ekf_euler_angles[t, 0] - true_euler_angles[t, 0])
        pitch_error = abs(ekf_euler_angles[t, 1] - true_euler_angles[t, 1])
        yaw_error = abs(ekf_euler_angles[t, 2] - true_euler_angles[t, 2])

        roll_rate_error = abs(ekf_angular_velocities[t, 0] - true_angular_velocities[t, 0])
        pitch_rate_error = abs(ekf_angular_velocities[t, 1] - true_angular_velocities[t, 1])
        yaw_rate_error = abs(ekf_angular_velocities[t, 2] - true_angular_velocities[t, 2])

        estimation_errors['roll'].append(roll_error)
        estimation_errors['pitch'].append(pitch_error)
        estimation_errors['yaw'].append(yaw_error)
        estimation_errors['roll_rate'].append(roll_rate_error)
        estimation_errors['pitch_rate'].append(pitch_rate_error)
        estimation_errors['yaw_rate'].append(yaw_rate_error)

        # 记录显著误差
        if t % 100 == 0:  # 每秒记录一次
            logger.info("时间 %.1fs: 横滚误差=%.2f°, 俯仰误差=%.2f°, 偏航误差=%.2f°",
                        current_time, roll_error, pitch_error, yaw_error)

# 计算性能指标
mean_errors = {key: np.mean(values) for key, values in estimation_errors.items()}
max_errors = {key: np.max(values) for key, values in estimation_errors.items()}
std_errors = {key: np.std(values) for key, values in estimation_errors.items()}

logger.info("模拟完成. 性能指标:")
logger.info("平均误差 - 横滚: %.3f°, 俯仰: %.3f°, 偏航: %.3f°",
            mean_errors['roll'], mean_errors['pitch'], mean_errors['yaw'])
logger.info("平均角速度误差 - 横滚: %.4f rad/s, 俯仰: %.4f rad/s, 偏航: %.4f rad/s",
            mean_errors['roll_rate'], mean_errors['pitch_rate'], mean_errors['yaw_rate'])
logger.info("最大误差 - 横滚: %.3f°, 俯仰: %.3f°, 偏航: %.3f°",
            max_errors['roll'], max_errors['pitch'], max_errors['yaw'])
logger.info("标准差 - 横滚: %.3f°, 俯仰: %.3f°, 偏航: %.3f°",
            std_errors['roll'], std_errors['pitch'], std_errors['yaw'])
logger.info("滤波器重置次数: %d", ekf.reset_count)

# 可视化
plt.figure(figsize=(15, 12))

# 横滚角
plt.subplot(3, 2, 1)
plt.plot(time_points, true_euler_angles[:, 0], label='真实横滚', linewidth=2)
plt.plot(time_points, ekf_euler_angles[:, 0], label='EKF估计', linestyle='--', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('横滚角 (deg)')
plt.title('横滚角估计\n(蓝色: 真实值, 橙色: EKF估计)')
plt.legend()
plt.grid(True)

# 俯仰角
plt.subplot(3, 2, 3)
plt.plot(time_points, true_euler_angles[:, 1], label='真实俯仰', linewidth=2)
plt.plot(time_points, ekf_euler_angles[:, 1], label='EKF估计', linestyle='--', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('俯仰角 (deg)')
plt.title('俯仰角估计\n(蓝色: 真实值, 橙色: EKF估计)')
plt.legend()
plt.grid(True)

# 偏航角
plt.subplot(3, 2, 5)
plt.plot(time_points, true_euler_angles[:, 2], label='真实偏航', linewidth=2)
plt.plot(time_points, ekf_euler_angles[:, 2], label='EKF估计', linestyle='--', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('偏航角 (deg)')
plt.title('偏航角估计\n(蓝色: 真实值, 橙色: EKF估计)')
plt.legend()
plt.grid(True)

# 角速度 - 横滚率
plt.subplot(3, 2, 2)
plt.plot(time_points, true_angular_velocities[:, 0], label='真实横滚率', linewidth=2)
plt.plot(time_points, gyro_measurements[:, 0], label='陀螺仪测量', alpha=0.7)
plt.plot(time_points, ekf_angular_velocities[:, 0], label='EKF估计', linestyle='--', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('横滚率 (rad/s)')
plt.title('横滚率估计\n(蓝色: 真实值, 绿色: 陀螺仪, 橙色: EKF)')
plt.legend()
plt.grid(True)

# 角速度 - 俯仰率
plt.subplot(3, 2, 4)
plt.plot(time_points, true_angular_velocities[:, 1], label='真实俯仰率', linewidth=2)
plt.plot(time_points, gyro_measurements[:, 1], label='陀螺仪测量', alpha=0.7)
plt.plot(time_points, ekf_angular_velocities[:, 1], label='EKF估计', linestyle='--', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('俯仰率 (rad/s)')
plt.title('俯仰率估计\n(蓝色: 真实值, 绿色: 陀螺仪, 橙色: EKF)')
plt.legend()
plt.grid(True)

# 角速度 - 偏航率
plt.subplot(3, 2, 6)
plt.plot(time_points, true_angular_velocities[:, 2], label='真实偏航率', linewidth=2)
plt.plot(time_points, gyro_measurements[:, 2], label='陀螺仪测量', alpha=0.7)
plt.plot(time_points, ekf_angular_velocities[:, 2], label='EKF估计', linestyle='--', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('偏航率 (rad/s)')
plt.title('偏航率估计\n(蓝色: 真实值, 绿色: 陀螺仪, 橙色: EKF)')
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.savefig('attitude_estimation.png', dpi=300, bbox_inches='tight')
plt.show()

# 误差分析图
plt.figure(figsize=(12, 8))

# 角度误差
plt.subplot(2, 1, 1)
error_time = time_points[11:]  # 跳过前几步
plt.plot(error_time, estimation_errors['roll'], label='横滚误差', alpha=0.8)
plt.plot(error_time, estimation_errors['pitch'], label='俯仰误差', alpha=0.8)
plt.plot(error_time, estimation_errors['yaw'], label='偏航误差', alpha=0.8)
plt.xlabel('时间 (s)')
plt.ylabel('角度误差 (deg)')
plt.title('姿态估计误差随时间变化')
plt.legend()
plt.grid(True)

# 角速度误差
plt.subplot(2, 1, 2)
plt.plot(error_time, estimation_errors['roll_rate'], label='横滚率误差', alpha=0.8)
plt.plot(error_time, estimation_errors['pitch_rate'], label='俯仰率误差', alpha=0.8)
plt.plot(error_time, estimation_errors['yaw_rate'], label='偏航率误差', alpha=0.8)
plt.xlabel('时间 (s)')
plt.ylabel('角速度误差 (rad/s)')
plt.title('角速度估计误差随时间变化')
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.savefig('estimation_errors.png', dpi=300, bbox_inches='tight')
plt.show()

# 偏置估计图
plt.figure(figsize=(12, 6))

plt.subplot(1, 3, 1)
plt.plot(time_points, ekf_biases[:, 0], label='横滚偏置估计', linewidth=2)
plt.axhline(y=0.01, color='r', linestyle='--', label='真实横滚偏置')
plt.xlabel('时间 (s)')
plt.ylabel('偏置 (rad/s)')
plt.title('横滚轴偏置估计')
plt.legend()
plt.grid(True)

plt.subplot(1, 3, 2)
plt.plot(time_points, ekf_biases[:, 1], label='俯仰偏置估计', linewidth=2)
plt.axhline(y=-0.01, color='r', linestyle='--', label='真实俯仰偏置')
plt.xlabel('时间 (s)')
plt.ylabel('偏置 (rad/s)')
plt.title('俯仰轴偏置估计')
plt.legend()
plt.grid(True)

plt.subplot(1, 3, 3)
plt.plot(time_points, ekf_biases[:, 2], label='偏航偏置估计', linewidth=2)
plt.axhline(y=0.005, color='r', linestyle='--', label='真实偏航偏置')
plt.xlabel('时间 (s)')
plt.ylabel('偏置 (rad/s)')
plt.title('偏航轴偏置估计')
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.savefig('bias_estimation.png', dpi=300, bbox_inches='tight')
plt.show()

# 线性加速度检测
if len(ekf.linear_acc_detection) > 0:
    plt.figure(figsize=(12, 4))
    plt.plot(ekf.linear_acc_detection)
    plt.axhline(y=1.0, color='r', linestyle='--', label='检测阈值')
    plt.xlabel('更新步骤')
    plt.ylabel('线性加速度检测值 (m/s²)')
    plt.title('线性加速度检测')
    plt.legend()
    plt.grid(True)
    plt.savefig('linear_acc_detection.png', dpi=300, bbox_inches='tight')
    plt.show()

# 创建性能指标汇总表
metrics_df = pd.DataFrame({
    '指标': ['平均误差', '最大误差', '标准差'],
    '横滚 (°)': [mean_errors['roll'], max_errors['roll'], std_errors['roll']],
    '俯仰 (°)': [mean_errors['pitch'], max_errors['pitch'], std_errors['pitch']],
    '偏航 (°)': [mean_errors['yaw'], max_errors['yaw'], std_errors['yaw']],
    '横滚率 (rad/s)': [mean_errors['roll_rate'], max_errors['roll_rate'], std_errors['roll_rate']],
    '俯仰率 (rad/s)': [mean_errors['pitch_rate'], max_errors['pitch_rate'], std_errors['pitch_rate']],
    '偏航率 (rad/s)': [mean_errors['yaw_rate'], max_errors['yaw_rate'], std_errors['yaw_rate']]
})

print("\n性能指标汇总:")
print(metrics_df.to_string(index=False))

# 曲线解释
print("\n" + "=" * 80)
print("曲线解释")
print("=" * 80)
print("1. 姿态角图 (左列):")
print("   - 蓝色曲线表示无人机的真实姿态角")
print("   - 橙色虚线表示EKF的估计值")
print("   - 两条曲线的接近程度反映了EKF的估计精度")
print("   - 快速机动时可能出现短暂偏差，但EKF应能快速收敛")

print("\n2. 角速度图 (右列):")
print("   - 蓝色曲线表示真实的角速度")
print("   - 绿色曲线表示带噪声的陀螺仪测量值")
print("   - 橙色虚线表示EKF的估计值")
print("   - EKF应能有效减少噪声并补偿陀螺仪偏置")

print("\n3. 偏置估计图:")
print("   - 这些图显示了EKF对陀螺仪偏置的估计")
print("   - 红色虚线表示真实的陀螺仪偏置")
print("   - EKF应能逐渐收敛到真实偏置值")

print("\n4. 误差图:")
print("   - 这些图显示了估计误差随时间的变化")
print("   - 稳定的低误差值表明滤波器性能良好")
print("   - 误差峰值通常对应于快速机动或外部扰动")
print("   - EKF应能快速从扰动中恢复")

print("\n5. 线性加速度检测图:")
print("   - 这个图显示了检测到的线性加速度大小")
print("   - 红色虚线表示检测阈值")
print("   - 超过阈值时，EKF会增加加速度计的测量噪声")

print("\n关键洞察:")
print("- EKF成功融合了多个传感器(陀螺仪、加速度计、磁力计)的数据")
print("- 它补偿了传感器偏置并减少了测量噪声")
print("- 滤波器在存在线性加速度的情况下仍能提供准确的态度估计")
print("- 性能指标显示了亚度级的态度估计精度")
print("- 角速度估计相比原始陀螺仪测量有显著改进")
print("- 偏置估计能够逐渐收敛到真实值")